/*
Copyright (c) [2019年5月1日] [吴超]
[MBT_studio] is licensed under Mulan PSL v2.
You can use this software according to the terms and conditions of the Mulan PSL v2.
You may obtain a copy of Mulan PSL v2 at:
		 http://license.coscl.org.cn/MulanPSL2
THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
See the Mulan PSL v2 for more details.
*/
#include "stdafx.h"
#include "体素坐标生成.h"


static float32 g防止除0补偿值 = 0.00000001f;





void f_初始化粒子刚体变换(mat3X3& 旋转矩阵, vec3& 中心坐标, vec3* points, float32* 质量, uint32 num) {
	中心坐标 = {};
	f_mat_zero(旋转矩阵);



	float32 权重和 = 0;
#pragma omp parallel for
	for (int64 i = 0; i < num; ++i) {
		float32 权重 = 1.0f / (质量[i] + g防止除0补偿值);
		vec_add(&中心坐标, vec_mul(points[i], 权重));
		//p_outData[i] = !p_dataA[i];
		权重和 += 权重;
	}

	if (权重和 != 0) {
		vec_div(&中心坐标, 权重和);
		//C_Mat33 rm;
		//rm.cols[0] = {};
		//rm.cols[1] = {};
		//rm.cols[2] = {};
#pragma omp parallel for
		for (int64 i = 0; i < num; ++i) {
			vec3 q = vec_sub(points[i], 中心坐标);
			float32 权重 = 1.0f / (质量[i] + 0.0000001);

			float32 x2 = 权重 * q.x * q.x;
			float32 y2 = 权重 * q.y * q.y;
			float32 z2 = 权重 * q.z * q.z;
			float32 xy = 权重 * q.x * q.y;
			float32 xz = 权重 * q.x * q.z;
			float32 yz = 权重 * q.y * q.z;

			旋转矩阵.row0.x += x2;
			旋转矩阵.row0.y += xy;
			旋转矩阵.row0.z += xz;

			旋转矩阵.row1.x += xy;
			旋转矩阵.row1.y += y2;
			旋转矩阵.row1.z += yz;

			旋转矩阵.row2.x += xz;
			旋转矩阵.row2.y += yz;
			旋转矩阵.row2.z += z2;

			/*旋转矩阵.row0.x += x2;
			旋转矩阵.row1.x += xy;
			旋转矩阵.row2.x += xz;

			旋转矩阵.row0.y += xy;
			旋转矩阵.row1.y += y2;
			旋转矩阵.row2.y += yz;

			旋转矩阵.row0.z += xz;
			旋转矩阵.row1.z += yz;
			旋转矩阵.row2.z += z2;*/


			/*rm.cols[0].x += x2;
			rm.cols[0].y += xy;
			rm.cols[0].z += xz;

			rm.cols[1].x += xy;
			rm.cols[1].y += y2;
			rm.cols[1].z += yz;

			rm.cols[2].x += xz;
			rm.cols[2].y += yz;
			rm.cols[2].z += z2;*/

			/*rm.cols[0].x += x2;
			rm.cols[1].x += xy;
			rm.cols[2].x += xz;

			rm.cols[0].y += xy;
			rm.cols[1].y += y2;
			rm.cols[2].y += yz;

			rm.cols[0].z += xz;
			rm.cols[1].z += yz;
			rm.cols[2].z += z2;*/
		}


		//f_求逆矩阵(rm);
		//f_mat_转换(旋转矩阵, rm);
	}
	else {
		中心坐标 = {};
	}


}





